#include <Eigen/Sparse>
#include <iostream>
#include <fstream>
#include "../include/Grid.hpp"


Eigen::VectorXd apply_constraint_solve(Eigen::SparseMatrix<double>& A, Eigen::VectorXd& b, Grid& g, std::function<double(double, double)> gd)
{
    unsigned long size_m = A.rows();
    Eigen::VectorXd diff = Eigen::VectorXd::Zero(size_m);
    Eigen::VectorXd truth = Eigen::VectorXd::Zero(size_m);
    std::cerr << "reconstruct matrix by applying boundary constraint" << std::endl;
    for(unsigned long row=0; row<size_m; row++)
    {
        if(is_boundary(g.vertex[row]) == 1)
        {
            double bound_value = gd(g.vertex[row][0], g.vertex[row][1]);
//            b[row] = A.coeff(row,row) * bound_value;
            b[row] = bound_value;
            for(unsigned long col=0; col< size_m; col++)
            {
                if(row == col)
                {
//                    continue;
                    A.coeffRef(row,col) = 1.0;
                }
                else
                {
                    b[col] -= A.coeff(col, row) * bound_value;
                    A.coeffRef(row, col) = 0.0;
                    A.coeffRef(col, row) = 0.0;
                }
            }
        }
        else
        {
            continue;
        }
    }
    A.makeCompressed();
     
    
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > solver;
//    solver.setTolerance(1e-8);
    solver.compute(A);
    Eigen::VectorXd x = solver.solve(b);
//    std::cout << (A * x-b) << std::endl; 
    
    // for(unsigned long idx=0; idx<size_m; idx++)
    // {
        // truth[idx] = gd(g.vertex[idx][0], g.vertex[idx][1]);
    // }
    // diff = truth - x;
    // std::cout << "unbearable error if we put truth as solution before apply constraint" << std::endl;
    
    // for(unsigned long idx=0; idx<size_m; idx++)
    // {
        // if(abs(diff[idx]) > 0.1)
        // {
            // std::cout << g.vertex[idx] << " has error=" << diff[idx] << std::endl;
        // }
    // }
    // std::ofstream mout;
    // mout.open("../solvers/matrix.txt");
    // for(unsigned long row=0; row<size_m; row++)
    // {
        // for(unsigned long col=0; col<size_m; col++)
        // {
            // mout << A.coeff(row,col) << " ";
        // }
        // mout << std::endl;
    // }
    // mout.close();
//
    // std::ofstream bout;
    // bout.open("../solvers/b.txt");
    // for(unsigned long i=0; i<size_m; i++)
    // {
        // bout << b(i) << std::endl;
    // }
    // bout.close();
//
    // std::ofstream xout;
    // xout.open("../solvers/x.txt");
    // for(unsigned long i=0; i<size_m; i++)
    // {
        // xout << x(i) << std::endl;
    // }
    // std::cerr << "solve linear eqautions successfully and store all data!" << std::endl;
    return x;
}
